﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.PhysicalModel;
using W3C.Soap;

using webcam = Microsoft.Robotics.Services.WebCam.Proxy;
using lrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
using drive = SimJaguar.QuadDifferentialDrive.Proxy;

namespace SimJaguars
{
     

    /// <summary>
    /// SimJaguars state
    /// </summary>
    [DataContract]
    public class SimJaguarsState
    {
        private Robot[] Jaguar = new Robot[SimJaguarsService.num_of_robots + 1];

        public void Initialize()
        {
            for (int i = 1; i < Jaguar.Length; i++)
                Jaguar[i] = new Robot();
        }

        [DataMember]
        [Description("Vector of robotic entities containing all data members included in Robot class")]
        public Robot[] jaguar
        {
            get { return Jaguar; }
            set { Jaguar = value; }
        }

    }

    [DataContract]
    public class Robot
    {

        private Sensors _Sensors = new Sensors();

        [DataMember]
        public Sensors Sensors
        {
            get { return _Sensors; }
            set { _Sensors = value; }
        }

        private Physics _Physics = new Physics();

        [DataMember]
        public Physics Pose
        {
            get { return _Physics; }
            set { _Physics = value; }
        }

    }



    [DataContract]
    public class Physics
    {
        [DataMember]
        public double Speed;

        [DataMember]
        public double Heading;

        [DataMember]
        public double X_pose;

        [DataMember]
        public double Y_pose;

        [DataMember]
        public double Z_pose;

        [DataMember]
        public Vector3 PositionVector;

        [DataMember]
        public Vector3 VelocityVector;

        [DataMember]
        public Quaternion OrientationVector;

        private double _LeftFrontSpeed, _LeftRearSpeed, _RightFrontSpeed, _RightRearSpeed;

        [DataMember]
        [Description("in rads/sec")]
        public double LeftFrontSpeed
        {
            get { return _LeftFrontSpeed; }
            set { _LeftFrontSpeed = value; }
        }

        [DataMember]
        [Description("in rads/sec")]
        public double LeftRearSpeed
        {
            get { return _LeftRearSpeed; }
            set { _LeftRearSpeed = value; }
        }

        [DataMember]
        [Description("in rads/sec")]
        public double RightFrontSpeed
        {
            get { return _RightFrontSpeed; }
            set { _RightFrontSpeed = value; }
        }

        [DataMember]
        [Description("in rads/sec")]
        public double RightRearSpeed
        {
            get { return _RightRearSpeed; }
            set { _RightRearSpeed = value; }
        }

    }


    [DataContract]
    public class Wheel
    {
        public double AxleSpeed;
    }

    [DataContract]
    public class Sensors
    {
        [DataMember]
        public webcam.QueryFrameResponse WebcamData;

        [DataMember]
        public Image WebcamBitmap;

        [DataMember]
        public lrf.State laserData;

    }
}
